//
// Created by whitby on 8/22/23.
//

#include "G2oTypes.h"
#include "Utils/LieAlgeBra.h"
#include "Sensor/Camera.h"

namespace mono_orb_slam2 {

    void EdgeMonoPose::linearizeOplus() {
        const auto *vPose = dynamic_cast<const VertexPose *>(_vertices[0]);
        const Eigen::Vector3d Pc = vPose->estimate().map(P_w);

        Eigen::Matrix<double, 2, 3> projectJacobian = camera->getProjJacobian(Pc);
        _jacobianOplusXi.block<2, 3>(0, 0) = projectJacobian * lie::Hat(Pc);
        _jacobianOplusXi.block<2, 3>(0, 3) = -projectJacobian;
    }

    void EdgeMono::linearizeOplus() {
        const auto *vPose = dynamic_cast<const VertexPose *>(_vertices[1]);
        g2o::SE3Quat Tcw(vPose->estimate());
        const auto *vPoint = dynamic_cast<const VertexPoint *>(_vertices[0]);

        Eigen::Vector3d Pc = Tcw.map(vPoint->estimate());
        Eigen::Matrix<double, 2, 3> projectJacobian = camera->getProjJacobian(Pc);

        _jacobianOplusXi = -projectJacobian * Tcw.rotation().toRotationMatrix();
        _jacobianOplusXj.block<2, 3>(0, 0) = projectJacobian * lie::Hat(Pc);
        _jacobianOplusXj.block<2, 3>(0, 3) = -projectJacobian;
    }

}
